#include "IterationMethods.h"

namespace NumericalAnalysis
{
    // Iteration method:
    // The parameters are:
    // Iteration method to use (Jacobi or GaussSeidel)
    // Matrix dimensions n
    // File path to read the matrix A (nxn).
    // File path to read the matrix b (nx1).
    // File path to read the matrix x (nx1).
    // Error tolerance
    // Max number of iterations
    // Also we need to pass three output parameters for:
    // Last iteration
    // First order norm value
    // Last iteration x matrix.
    void IterationMethod(const Method method, const uint32_t matrixDimension, const std::string& filePathForA, 
    	const std::string& filePathForb, const std::string& filePathForx, const double errorTolerance, 
    	const uint32_t maxIterations, uint32_t& outLastIteration, double& outFirstOrderNorm, Utils::Matriz& outMatrix)
    {
    	// Execute the correct iteration method.
    	if(method == Method_Jacobi)
    		JacobiMethod(matrixDimension, filePathForA, filePathForb, filePathForx, errorTolerance,
    		maxIterations, outLastIteration, outFirstOrderNorm, outMatrix);
    		
    	else if(method == Method_GaussSeidel)
    		GaussSeidelMethod(matrixDimension, filePathForA, filePathForb, filePathForx, errorTolerance, 
    		maxIterations, outLastIteration, outFirstOrderNorm, outMatrix);
    }

    // Jacobi method:
    // The parameters are:
    // Matrix dimensions n
    // File path to read the matrix A (nxn).
    // File path to read the matrix b (nx1).
    // File path to read the matrix x (nx1).
    // Error tolerance
    // Max number of iterations
    // Also we need to pass two output parameters for:
    // Last iteration
    // First order norm value
    void JacobiMethod(const uint32_t matrixDimension, const std::string& filePathForA, const std::string& filePathForb, 
    	const std::string& filePathForx, const double errorTolerance, const uint32_t maxIterations, uint32_t& outLastIteration, 
    	double& outFirstOrderNorm, Utils::Matriz& outMatrix)
    {
    	Utils::Matriz A = Utils::GetMatrixFromFile(filePathForA, matrixDimension, matrixDimension);
    	Utils::Matriz b = Utils::GetMatrixFromFile(filePathForb, matrixDimension, 1);
    	Utils::Matriz x = Utils::GetMatrixFromFile(filePathForx, matrixDimension, 1);

    	Utils::Matriz previousX(matrixDimension, 1);

    	for(uint32_t k = 1; k <= maxIterations; ++k)
    	{
    		previousX = x;

    		Utils::Matriz u(matrixDimension, matrixDimension);
    		
			// - Sumatory[j = 1, j != i, n] (Aij / Aii) * Xj(k - 1)
			// +
			// ( bi / Aii )
    		for(uint32_t i = 0; i < matrixDimension; ++i)
    		{
				// Sumatory[j = 1, j != i, n] Aij * Xj(k - 1)
    			double sum = 0.0;
    			for(uint32_t j = 0; j < matrixDimension; ++j)
    			{
    				if(j != i)
    					sum += A.mData[j + i * A.mColumns] * x.mData[j];
    			}

				// (bi - sum) / Aii
    			u.mData[i] = (b.mData[i] - sum) / A.mData[i + i * A.mRows];
    		}

			// Update x
    		for(uint32_t i = 0; i < matrixDimension; ++i)
    			x.mData[i] = u.mData[i];

			// || Xk - X(k - 1) ||
    		const double firstOrderNorm = Utils::GetFirstOrderVectorNorm(Utils::AddMatrices(x, 
    		                              Utils::MultiplyMatrixByLiteral(previousX, -1.0) ) ); 
    		if(firstOrderNorm <= errorTolerance)
    		{
    			outFirstOrderNorm = firstOrderNorm;
    			outLastIteration = k;
    			outMatrix = x;
    			
    			return;
    		}
    	}

    	outLastIteration = maxIterations;
    	outFirstOrderNorm = Utils::GetFirstOrderVectorNorm( Utils::AddMatrices(x, Utils::MultiplyMatrixByLiteral(previousX, -1.0f) ) );
    	outMatrix = x;
    }

    // Gauss-Seidel method:
    // The parameters are:
    // Matrix dimensions n
    // File path to read the matrix A (nxn).
    // File path to read the matrix b (nx1).
    // File path to read the matrix x (nx1).
    // Error tolerance
    // Max number of iterations
    // Also we need to pass three output parameters for:
    // Last iteration
    // First order norm value
    // Last iteration x matrix.
    void GaussSeidelMethod(const uint32_t matrixDimension, const std::string& filePathForA, const std::string& filePathForb, 
    	const std::string& filePathForx, const double errorTolerance, const uint32_t maxIterations, uint32_t& outLastIteration, 
    	double& outFirstOrderNorm, Utils::Matriz& outMatrix)
    {

    	Utils::Matriz A = Utils::GetMatrixFromFile(filePathForA, matrixDimension, matrixDimension);
    	Utils::Matriz b = Utils::GetMatrixFromFile(filePathForb, matrixDimension, 1);
    	Utils::Matriz x = Utils::GetMatrixFromFile(filePathForx, matrixDimension, 1);

    	Utils::Matriz previousX(matrixDimension, 1);

    	for(uint32_t k = 1; k <= maxIterations; ++k)
    	{
    		previousX = x;

			// - Sumatory[j = 1, j < i, n] (Aij / Aii) * Xj(k)
			// - Sumatory[j = 1, i < j, n] (Aij / Aii) * Xj(k - 1)
			// +
			// ( bi / Aii )
    		for(uint32_t i = 0; i < matrixDimension; ++i)
    		{
				// - Sumatory[j = 1, j < i, n] (Aij / Aii) * Xj(k)
				// - Sumatory[j = 1, i < j, n] (Aij / Aii) * Xj(k - 1)
    			double sum = 0.0;
    			for(uint32_t j = 0; j < matrixDimension; ++j)
    			{
					if (j < i)
						sum += A.mData[j + i * A.mRows] * x.mData[j];
					else if (i < j)
						sum += A.mData[j + i * A.mRows] * previousX.mData[j];
    			}

				// Xi = (bi - sum) / Aii
    			x.mData[i] = (b.mData[i] - sum) / A.mData[i + i * A.mRows];

    		}

			// || Xk - X(k - 1) ||
    		const double firstOrderNorm = Utils::GetFirstOrderVectorNorm(Utils::AddMatrices(x, 
    		                              Utils::MultiplyMatrixByLiteral(previousX, -1.0) ) ); 

    		if(firstOrderNorm <= errorTolerance)
    		{
    			outFirstOrderNorm = firstOrderNorm;
    			outLastIteration = k;
    			outMatrix = x;
    		
    			return;
    		}
    	}

    	outLastIteration = maxIterations;
    	outFirstOrderNorm = Utils::GetFirstOrderVectorNorm( Utils::AddMatrices(x, Utils::MultiplyMatrixByLiteral(previousX, -1.0) ) );
    	outMatrix = x;
    }   
}
